"""
#include <stdio.h>

float setValue=0;
float pid(float setpoint,float measured_value){
	float Kp=13,Ki=0.01,Kd=0;
	float output =0;
	static float previous_error=20;//当前误差
	static float error=20;//上次误差
	static float intergral=20;//积分
	static float derivative=20;//微分
	
	
	error = setpoint - measured_value;//上次偏差
	intergral+=error;//积分和
	derivative=error -previous_error;//微分
	output=Kp*error+Ki*intergral+Kd*derivative;//输出
	
	previous_error = error;//当前误差
	return output;
}

/*限幅函数*/
int PWM_MAX=7200,PWM_MIN=-7200;	//PWM限幅变量
void Limit(int *motoA,int *motoB)
{
	if(*motoA>PWM_MAX)*motoA=PWM_MAX;
	if(*motoA<PWM_MIN)*motoA=PWM_MIN;
	if(*motoB>PWM_MAX)*motoB=PWM_MAX;
	if(*motoB<PWM_MIN)*motoB=PWM_MIN;
}



int main()
{
   /*  Write C code in this online editor and run it. */
	
   printf("Hello, World! \n");
   //printf("output:%f\r\n",pid(30,15));
   while(1);
   
   return 0;
}
"""

Kp=10.0
Ki=1
Kd=0

previous_error=0.0
error=0.0
intergral=0.0
derivative=0.0

def pid(setpoint,measured_value):
    #global out,Kp,Ki,Kd
    global previous_error,error,intergral,derivative
    error = setpoint - measured_value
    intergral+=error
    derivative=error -previous_error
    output=Kp*error+Ki*intergral+Kd*derivative

    previous_error = error
    return output

